#include <iostream>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <X11/Xlib.h>
#include "Driver/HKVideoCapture.hpp"
#include "Driver/HKMutiCam.hpp"
#include "Windmill/Windmill.h"
#include "Armor/ArmorDetector.h"
#include "Pose/AngleSolver.hpp"

using namespace std;
using namespace cv;
using namespace rm;


/*事先标定好的左相机的内参矩阵
fx 0 cx
0 fy cy
0  0  1
*/
Mat cameraMatrixL = (Mat_<float>(3, 3) << 1853.2, 0, 632.0188,
    0, 1853.0, 510.4157,
    0, 0, 1);
//获得的畸变参数


Mat distCoeffL = (Mat_<float>(5, 1) << -0.1588, 0.1624, 0, 0, 0);


/*事先标定好的右相机的内参矩阵
fx 0 cx
0 fy cy
0  0  1
*/
Mat cameraMatrixR = (Mat_<float>(3, 3) << 1855.7, 0, 670.8712,
    0, 1855.8, 515.2617,
    0, 0, 1);


Mat distCoeffR = (Mat_<float>(5, 1) << -0.1570, 0.1626, 0, 0, 0);

Mat T = (Mat_<float>(3, 1) << -263.4404, -0.5599, 1.8181);//T平移向量

Mat R = (Mat_<float>(3, 3) << 0.9999, -0.0018, 0.0106,
    0.0017, 0.9999, 0.0115,
    -0.0106, -0.0115, 0.9999);                //rec旋转矩阵，转置



void triangulate_solve(const cv::Point2f& p_left, const cv::Point2f& p_right,
    const cv::Mat& m_left, const cv::Mat& m_right, const cv::Mat& R, const cv::Mat& T, cv::Point3f& target3D)
{
    Mat proj1(3, 4, CV_32FC1);
    Mat proj2(3, 4, CV_32FC1);

    proj1(Range(0, 3), Range(0, 3)) = Mat::eye(3, 3, CV_32FC1);
    proj1.col(3) = Mat::zeros(3, 1, CV_32FC1);

    R.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);
    T.convertTo(proj2.col(3), CV_32FC1);

    Mat fm1,fm2;
    m_left.convertTo(fm1, CV_32FC1);
    m_right.convertTo(fm2, CV_32FC1);

    proj1 = fm1 * proj1;
    proj2 = fm2 * proj2;


    //三角化重建
    Mat structure;
    vector<Point2f> p1, p2;
    p1.push_back(p_left);
    p2.push_back(p_right);
    triangulatePoints(proj1, proj2, p1, p2, structure);

    Mat_<float> col = structure.col(0);
    col /= col(3);

    target3D.x = col(0) + 0.5*T.at<float>(0, 0);
    target3D.y = col(1);
    target3D.z = col(2);
}

int main()
{
    XInitThreads();
    HKMutiCam HKCamera;
    vector<Mat> srcs;

    std::unique_ptr<ArmorDetector> _larmorDetectorPtr;
    _larmorDetectorPtr = make_unique<ArmorDetector>();
    std::unique_ptr<ArmorDetector> _rarmorDetectorPtr;
    _rarmorDetectorPtr = make_unique<ArmorDetector>();
    int self_color;
    //self_color = rm::BLUE;
    self_color = rm::RED;
    ArmorParam armorParam;
    _larmorDetectorPtr->init(armorParam);
    _larmorDetectorPtr->setEnemyColor(self_color == rm::BLUE ? rm::RED : rm::BLUE, 0);
    _rarmorDetectorPtr->init(armorParam);
    _rarmorDetectorPtr->setEnemyColor(self_color == rm::BLUE ? rm::RED : rm::BLUE, 1);

    int larmorFlag;
    int larmorType;
    std::vector<cv::Point2f> lLightbarVertex;
    Point2f lCenter;

    int rarmorFlag;
    int rarmorType;
    std::vector<cv::Point2f> rLightbarVertex;
    Point2f rCenter;


    while(1)
    {
        bool isEmpty = 0;
        bool left_find = 0;
        bool right_find = 0;
        HKCamera.ReadImg(srcs);
        for(int i = 0; i < CAMERA_NUM; i++)
        {
            if(srcs.at(i).empty())isEmpty = 1;
            if(srcs.at(i).empty())continue;
            //imshow(to_string(i), srcs[i]);
        }
        if(isEmpty == 1)continue;

//        if(waitKey(15) == 32)
//        {
//            HKCamera.saveImg();
//        }

        _larmorDetectorPtr->loadImg(srcs.at(0));
        larmorFlag = _larmorDetectorPtr->detect();
//        cout<<"larmorFlag"<<larmorFlag<<endl;
        if(larmorFlag == ArmorDetector::ARMOR_LOCAL || larmorFlag == ArmorDetector::ARMOR_GLOBAL || larmorFlag == ArmorDetector::TRACKING)
        {
            lLightbarVertex = _larmorDetectorPtr->getLightbarVertex();
            larmorType = _larmorDetectorPtr->getArmorType();

            if(larmorFlag == ArmorDetector::TRACKING)
            {
                if(_larmorDetectorPtr->_para._losting)
                {
                    cout<<"taget is losting..."<<endl;
                    continue;
                }
                lCenter = _larmorDetectorPtr->getKCFTarget();
            }
            else
            {
                lCenter =_larmorDetectorPtr->getTargetCenter();
            }
            left_find = 1;
        }

        Rect right_roi;
        if(_larmorDetectorPtr->get_roi().width != 0)
        {
            right_roi = _larmorDetectorPtr->get_roi();
        }
        else
        {
            right_roi = Rect(Point(0,0), srcs.at(1).size());
        }

        _rarmorDetectorPtr->loadImg(srcs.at(1)/*(right_roi)*/.clone());
        rarmorFlag = _rarmorDetectorPtr->detect();

        if(rarmorFlag == ArmorDetector::ARMOR_LOCAL || rarmorFlag == ArmorDetector::ARMOR_GLOBAL || rarmorFlag == ArmorDetector::TRACKING)
        {
            rLightbarVertex = _rarmorDetectorPtr->getLightbarVertex();
            rarmorType = _rarmorDetectorPtr->getArmorType();

            if(rarmorFlag == ArmorDetector::TRACKING)
            {
                if(_rarmorDetectorPtr->_para._losting)
                {
                    cout<<"taget is losting..."<<endl;
                    continue;
                }
                rCenter = _rarmorDetectorPtr->getKCFTarget();
            }
            else
            {
                rCenter =_rarmorDetectorPtr->getTargetCenter();
            }
            right_find = 1;
            //rCenter += Point2f(right_roi.tl().x, right_roi.tl().y);
        }

        if(left_find && right_find)
        {
            Point2f ludCenter;
            vector<cv::Point2f> lin;
            vector<cv::Point2f> lout;
            lin.push_back(lCenter);
            //对像素点去畸变
            undistortPoints(lin,lout,cameraMatrixL,distCoeffL,noArray(),cameraMatrixL);
            ludCenter=lout.front();
            //ludCenter = Point2f(701, 503);

            Point2f rudCenter;
            vector<cv::Point2f> rin;
            vector<cv::Point2f> rout;
            rin.push_back(rCenter);
            //对像素点去畸变
            undistortPoints(rin,rout,cameraMatrixR,distCoeffR,noArray(),cameraMatrixR);
            rudCenter=rout.front();
            //rudCenter = Point2f(546, 528);


            cout<<ludCenter<<rudCenter<<endl;
            Point3f pts3D;
            triangulate_solve(ludCenter, rudCenter, cameraMatrixL, cameraMatrixR, R, T, pts3D);
            cout<<"target: ["<<pts3D.x<<", "<<pts3D.y<<", "<<pts3D.z<<"]"<<endl;
            cout<<"dis:  "<<sqrt(pts3D.x*pts3D.x + pts3D.y*pts3D.y + pts3D.z*pts3D.z)<<endl;
            cout<<"angle: "<<atan(pts3D.x/pts3D.z)*180/CV_PI<<"  "<<atan(pts3D.y/pts3D.z)*180/CV_PI<<endl;

            double xl, yl;
            xl = ludCenter.x;
            yl = ludCenter.y;
            double fxl = cameraMatrixL.at<float>(0,0);
            double fyl = cameraMatrixL.at<float>(1,1);
            double cxl = cameraMatrixL.at<float>(0,2);
            double cyl = cameraMatrixL.at<float>(1,2);

            double rxlNew=(xl-cxl)/fxl;
            double rylNew=(yl-cyl)/fyl;

            double _xErrl=atan(rxlNew)/CV_PI*180;
            double _yErrl=atan(rylNew)/CV_PI*180;

            double xr, yr;
            xr = rudCenter.x;
            yr = rudCenter.y;
            double fxr = cameraMatrixR.at<float>(0,0);
            double fyr = cameraMatrixR.at<float>(1,1);
            double cxr = cameraMatrixR.at<float>(0,2);
            double cyr = cameraMatrixR.at<float>(1,2);

            double rxrNew=(xr-cxr)/fxr;
            double ryrNew=(yr-cyr)/fyr;

            double _xErrr=atan(rxrNew)/CV_PI*180;
            double _yErrr=atan(ryrNew)/CV_PI*180;

            cout<<"angle___: "<<0.5*(_xErrl+_xErrr)<<"  "<<0.5*(_yErrl+_yErrr)<<endl;
            cout<<"xErrl"<<_xErrl<<endl;
            cout<<"xErrr"<<_xErrr<<endl<<endl;
        }

    }
}
